#ifndef LINE_ALIGNMENT__LINE_ALIGNER_HPP
#define LINE_ALIGNMENT__LINE_ALIGNER_HPP

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/bool.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include "builtin_interfaces/msg/time.hpp"
#include "line_alignment/vel_json.hpp"
#include <chrono>
#include <cmath>
#include <memory>
#include <vector>
#include <string>
#include <mutex>
#include <algorithm>
#include <limits>


namespace line_alignment{

    using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

    // 状态机状态枚举
    enum ControlState {
        IDLE,
        MOVE,
        SAMPLING,
        SIDEMOVE,
        VERIFYING,
        COMPLETED
    };

    // 参数结构体
    struct AlignmentParams {
        int controlling_period;
        double n_x_limit_max;
        double n_x_limit_min;
        double n_y_limit_max;
        double n_y_limit_min;
        double g_x_limit_max;
        double g_x_limit_min;
        double g_y_limit_max;
        double g_y_limit_min;
        std::string source_type;
        double max_velocity;
        double reaching_tolerance;
        double sampling_duration_sec;
        double SIGMA_THRESHOLD_BOUND_;
        double max_runtime_sec;
        // 话题参数
        std::string pointcloud_topic;
        std::string cmd_vel_topic;
        std::string completed_topic;

    };

    // 样本点云范围值
    struct SampleRange {
        double x_min;
        double x_max;
        double y_min;
        double y_max;

        // 提供构造函数,初始化为无效值或零
        SampleRange() : x_min(0.0), x_max(0.0), y_min(0.0), y_max(0.0) {}

        // 提供参数化构造函数
        SampleRange(double x_min_val, double x_max_val, double y_min_val, double y_max_val)
            : x_min(x_min_val), x_max(x_max_val), y_min(y_min_val), y_max(y_max_val) {}

        // 提供成员函数访问值
        double getXMin() const { return x_min; }
        double getXMax() const { return x_max; }
        double getYMin() const { return y_min; }
        double getYMax() const { return y_max; }
    };

    class LineAligner : public rclcpp_lifecycle::LifecycleNode
    {
        public:
            explicit LineAligner(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

        private:

            CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
            CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
            CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
            CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;

            // 回调函数
            void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
            void controlLoop();

            void declare_parameters();

            // 状态处理函数
            void handleIdleState();
            void handleMoveState();
            void handleSamplingState();
            void handleSideMoveState();
            void handleVerifyingState();
            void handleCompletedState();

            // 速度发布函数
            void publishVelocity();
            void stopRobot();
            void publishCompleted(bool success);
            void Reset();

            // 采样函数
            void startSampling();
            void processSamples();
            std::tuple<std::vector<double>, std::vector<double>, std::vector<double>, std::vector<double>>
                removeOutliersBounds(const std::vector<double>& x_min_coords,
                                    const std::vector<double>& x_max_coords,
                                    const std::vector<double>& y_min_coords,
                                    const std::vector<double>& y_max_coords);
            
            
            // 订阅发布
            rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
            std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> cmd_vel_pub_;
            std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>> completed_pub_;
            // 定时器
            rclcpp::TimerBase::SharedPtr control_timer_;

            // 保护点云边界数据的互斥锁
            mutable std::mutex cloud_bounds_mutex_;

            // JSON类实体
            VelocityJsonConverter vel_cmd;

            // 状态变量
            bool pointcloud_received_;
            ControlState control_state_;
            AlignmentParams params_;
            std::chrono::duration<double> sampling_duration_;

            // 样本值存储
            std::vector<SampleRange> sample_range_;
            SampleRange current_range_;
            rclcpp::Time sampling_start_time_;
            
            // 点云参数
            double cloud_x_max;
            double cloud_x_min;
            double cloud_y_max;
            double cloud_y_min;

            double x_limit_min;
            double x_limit_max;
            double y_limit_min;
            double y_limit_max;


            // 取样平均参数
            double x_avg_max;
            double x_avg_min;
            double y_avg_max;
            double y_avg_min;


            // 速度参数
            double linear_x;
            double linear_y;
            double angular_z;

            // 添加状态标志
            bool state_is_active_;

    };


}

#endif
